/// 给定A坐标3点和B坐标3点，计算旋转矩阵+平移
/// 采用EulerZYX 方法， 有3个角度
/// TODO: 这个是失败的例子
#include "ceres/ceres.h"
#include "glog/logging.h"

#include <memath/meTransform.h>
#include <memath/MeEigenMath.h>

using namespace std;

class ProjMCostFunction
    : public ceres::SizedCostFunction<
        3 /* number of residuals */,
        3 /* size of first parameter */> {
 public:
    ProjMCostFunction(const Eigen::Vector3d &p1, const Eigen::Vector3d &p2):
        pI(p1), pO(p2){}
    virtual ~ProjMCostFunction() {}

    virtual bool Evaluate(double const* const* parameters,
                          double* residuals,
                          double** jacobians) const {
        double x = parameters[0][0];
        double y = parameters[0][1];
        double z = parameters[0][2];

        double px = cos(y)*cos(z)*pI[0] + (sin(x)*sin(y)*cos(z) - cos(x)*sin(z))*pI[1] + (cos(x)*sin(y)*cos(z) + sin(x)*sin(z))*pI[2];
        double py = cos(y)*sin(z)*pI[0] + (sin(x)*sin(y)*sin(z) + cos(x)*cos(z))*pI[1] + (cos(x)*sin(y)*sin(z) - sin(x)*cos(z))*pI[2];
        double pz = -sin(y)*pI[0] + (sin(x)*cos(y))*pI[1] + (cos(x)*cos(y))*pI[2];

        residuals[0] = px - pO[0];
        residuals[1] = py - pO[1];
        residuals[2] = pz - pO[2];

        // f'(x) = -1. Since there's only 1 parameter and that parameter
        // has 1 dimension, there is only 1 element to fill in the
        // jacobians.
        //
        // Since the Evaluate function can be called with the jacobians
        // pointer equal to NULL, the Evaluate function must check to see
        // if jacobians need to be computed.
        //
        // For this simple problem it is overkill to check if jacobians[0]
        // is NULL, but in general when writing more complex
        // CostFunctions, it is possible that Ceres may only demand the
        // derivatives w.r.t. a subset of the parameter blocks.
        if (jacobians != NULL && jacobians[0] != NULL) {
            jacobians[0][0] = (cos(x)*sin(y)*cos(z) + sin(x)*sin(z))*pI[1] + (-sin(x)*sin(y)*cos(z) + cos(x)*sin(z))*pI[2];
            jacobians[0][1] = -sin(y)*cos(z)*pI[0] + (sin(x)*cos(y)*cos(z))*pI[1] + (cos(x)*cos(y)*cos(z))*pI[2];;
            jacobians[0][2] = -cos(y)*sin(z)*pI[0] + (-sin(x)*sin(y)*sin(z) - cos(x)*cos(z))*pI[1] + (-cos(x)*sin(y)*sin(z) + sin(x)*cos(z))*pI[2];

            jacobians[0][3 + 0] = (cos(x)*sin(y)*sin(z) - sin(x)*cos(z))*pI[1] + (-sin(x)*sin(y)*sin(z) + cos(x)*cos(z))*pI[2];
            jacobians[0][3 + 1] = -sin(y)*sin(z)*pI[0] + (sin(x)*cos(y)*sin(z))*pI[1] + (cos(x)*cos(y)*sin(z))*pI[2];
            jacobians[0][3 + 2] = cos(y)*cos(z)*pI[0] + (sin(x)*sin(y)*cos(z) - cos(x)*sin(z))*pI[1] + (cos(x)*sin(y)*cos(z) + sin(x)*sin(z))*pI[2];

            jacobians[0][6 + 0] =  (cos(x)*cos(y))*pI[1] + (-sin(x)*cos(y))*pI[2];
            jacobians[0][6 + 1] = -cos(y)*pI[0] + (-sin(x)*sin(y))*pI[1] + (-cos(x)*sin(y))*pI[2];
            jacobians[0][6 + 2] = 0;
        }

        return true;
    }

    const Eigen::Vector3d pI;
    const Eigen::Vector3d pO;
};


int main(int argc, char** argv) {
    google::InitGoogleLogging(argv[0]);

    // 生成数据
    Matrix4d m = MeTransform::rotateZ(30*ME_DEGREE2RAD)
            *MeTransform::rotateY(20*ME_DEGREE2RAD)
            *MeTransform::rotateX(10*ME_DEGREE2RAD);

    vector<Eigen::Vector3d> p_I = {
        {1, 1, 1},
        {1, 1, 2},
        {2, 1, 1},
        {2, 0, 0},
    };

    vector<Eigen::Vector3d> p_O;
    for(uint i=0; i< p_I.size(); i++) {
        Eigen::Vector3d pO = MeEigenMath::multiplyVector3d(m, p_I[i]);
        p_O.push_back(pO);
    }


    // Build the problem.
    ceres::Problem problem;

    double parameters[3] = {10*ME_DEGREE2RAD, 20*ME_DEGREE2RAD, 20*ME_DEGREE2RAD};

    // Set up the only cost function (also known as residual).
    for (uint i=0; i<p_I.size();i++) {
        ceres::CostFunction* cost_function = new ProjMCostFunction(p_I[i], p_O[i]);
        problem.AddResidualBlock(cost_function, NULL, parameters);
    }


    // Run the solver!
    ceres::Solver::Options options;
    options.minimizer_progress_to_stdout = true;
    options.gradient_tolerance = 1e-16;
    options.function_tolerance = 1e-16;
    options.linear_solver_type = ceres::DENSE_QR;
    ceres::Solver::Summary summary;
    Solve(options, &problem, &summary);

    //std::cout << summary.BriefReport() << "\n";
    std::cout << summary.FullReport() << "\n";

    std::cout << "Expect   x: " << 10*ME_DEGREE2RAD << " y: " << 20*ME_DEGREE2RAD << " z: " << 30*ME_DEGREE2RAD
              << "\n";
    std::cout << "Final   x: " << parameters[0] << " y: " << parameters[1] << " z: " << parameters[2]
              << "\n";

    Matrix4d m2 = MeTransform::rotateZ(parameters[2])
            *MeTransform::rotateY(parameters[1])
            *MeTransform::rotateX(parameters[0]);

    cout << "m:\n" << m << endl;
    cout << "m2:\n" << m2 << endl;
    return 0;
}
